/**
 ****************************************************************************************************
 * @file        foc_hfi.c
 * @author      哔哩哔哩-Rebron大侠
 * @version     V0.0
 * @date        2025-01-11
 * @brief       高频注入相关代码
 * @license     MIT License
 *              Copyright (c) 2025 Reborn大侠
 *              允许任何人使用、复制、修改和分发该代码，但需保留此版权声明。
 ****************************************************************************************************
 */

#include "foc_hfi.h"
#include "Data_MATH.h"
#include "d_math.h"
#include "foc_filter.h"
#include <math.h>

#define D_AXLE_BIAS_VOLTAGE 1.5f /*用于充磁的D轴偏置电压*/

/*提取qd轴低频电流分量*/
I_dq_t FOC_HFI_Extract_LF(I_dq_t Iqd, I_dq_t Iqd_last)
{
    I_dq_t Iqd_lf = {0};

    Iqd_lf.i_q = (Iqd.i_q + Iqd_last.i_q) * 0.5f;
    Iqd_lf.i_d = (Iqd.i_d + Iqd_last.i_d) * 0.5f;

    return Iqd_lf;
}

/*提取qd轴高频电流分量*/
I_dq_t FOC_HFI_Extract_HF(I_dq_t Iqd, I_dq_t Iqd_last)
{
    I_dq_t Iqd_hf = {0};

    Iqd_hf.i_q = (Iqd.i_q - Iqd_last.i_q) * 0.5f;
    Iqd_hf.i_d = (Iqd.i_d - Iqd_last.i_d) * 0.5f;

    return Iqd_hf;
}

/*提取αβ轴低频电流分量*/
I_alphabeta_t FOC_HFI_alphabeta_LF(I_alphabeta_t Ialphabeta, I_alphabeta_t Ialphabeta_last)
{
    I_alphabeta_t alphabeta_lf = {0};

    alphabeta_lf.i_alpha = (Ialphabeta.i_alpha + Ialphabeta_last.i_alpha) * 0.5f; /*两次的高频分量相+=0*/
    alphabeta_lf.i_beta = (Ialphabeta.i_beta + Ialphabeta_last.i_beta) * 0.5f;

    return alphabeta_lf;
}

/*提取αβ轴高频电流分量*/
I_alphabeta_t FOC_HFI_alphabeta_HF(I_alphabeta_t Ialphabeta, I_alphabeta_t Ialphabeta_last)
{
    I_alphabeta_t alphabeta_hf = {0}; /*高频分量*/

    alphabeta_hf.i_alpha = (Ialphabeta.i_alpha - Ialphabeta_last.i_alpha) * 0.5f; /*两次的高频分量相-= 2倍绝对值*/
    alphabeta_hf.i_beta = (Ialphabeta.i_beta - Ialphabeta_last.i_beta) * 0.5f;

    return alphabeta_hf;
}

/*高频注入 + 极性辨识
给d轴添加高频电压, 从d轴正方向注入的话 Vd的响应电流幅值应该比-Vd要大
*/
void FOC_HFI_InjectPolIdent(FOC_HFI_t* hfi)
{
    if(hfi->hfi_time >= hfi->hfi_time_ref)
    {
        hfi->sign = -hfi->sign; /*电压取反*/
        hfi->hfi_time = 0;
        hfi->hfi_vol = (float)hfi->sign * hfi->hfi_vol_ref; /*极性辨识的时候不能充磁*/
    }
    else
    {
        hfi->hfi_time++;
    }

    /*需要高频注入角度已经收敛，且电机停止状态才能进行极性辨识*/
    if(fabsf(hfi->Encoder.ElAngle - hfi->last_ElAngle) <= 0.05f)
    {
        hfi->sum_count0++;
    }
    else
    {
        hfi->sum_count0 = 0;
        hfi->sum_count = 0;
        hfi->Id_lf_sum[0] = 0;
        hfi->Id_lf_sum[1] = 0;
    }
    hfi->last_ElAngle = hfi->Encoder.ElAngle;

    if(hfi->sum_count0 < 10000)
        return;

    /*帕克变换 得到高频qd轴电流*/
    Circular_t Circular = CORDIC_sin_cos(hfi->Encoder.ElAngle);
    hfi->Iqd_hf = Park(&hfi->alphabeta_h, &Circular);

    if(hfi->hfi_vol > 0)
    { /*上面取过一次反了，所以这里上次注入的是负电压*/
        hfi->Id_lf_sum[1] += fabsf(hfi->Iqd_hf.i_d);
    }
    else
    {
        hfi->Id_lf_sum[0] += fabsf(hfi->Iqd_hf.i_d);
    }
    hfi->sum_count++;

    if(hfi->sum_count >= 10000)
    {
        hfi->polarityFlag = true; /*极性辨识已完成*/
    }
}

/*高频注入*/
void FOC_HFI_Inject(FOC_HFI_t* hfi)
{
    if(hfi->hfi_time >= hfi->hfi_time_ref)
    {
        hfi->sign = -hfi->sign; /*电压取反*/
        hfi->hfi_time = 0;
        hfi->hfi_vol = (float)hfi->sign * hfi->hfi_vol_ref + hfi->d_bias_vol; /*高频电压  +d轴偏置*/
    }
    else
    {
        hfi->hfi_time++;
    }
}

/*高频初始化
hfi_vol_ref:    注入电压幅值
d_bias_vol:     d轴偏置电流幅值
hfi_run_freq:   高频注入函数运行的频率
hfi_freq:       高频注入频率
pole_pairs：    电机极对数
*/
void FOC_HFI_Init(FOC_HFI_t* hfi, float hfi_vol_ref, float d_bias_vol, uint32_t hfi_run_freq, uint32_t hfi_freq,
                  uint32_t pole_pairs)
{
    hfi->sign = 1;
    hfi->hfi_vol_ref = hfi_vol_ref;
    hfi->hfi_run_freq = hfi_run_freq;
    hfi->hfi_freq = hfi_freq;

    hfi->hfi_time_ref = hfi_run_freq / hfi_freq / 2;
    hfi->hfi_vol = hfi->hfi_vol_ref;

    hfi->d_bias_vol = d_bias_vol;

    hfi->Encoder.pole_pairs = pole_pairs;

    PLL_Init(&hfi->pll, 1.0f / (float)hfi_run_freq);
    // hfi->pll.Ki *= (1.0f/hfi_run_freq) ;

    // hfi->pll.Kp = 521.0f;
    // hfi->pll.Ki = 40000.0f;
}

/*高频角度计算*/
/*高频注入获取角度*/
void FOC_HFI_Angle_Calc(FOC_HFI_t* hfi, I_alphabeta_t alphabeta)
{
    I_alphabeta_t alphabeta_Ih = {0}; /*包络检测后的αβ*/

    int sign;
    if(hfi->hfi_vol > 0)
    {
        sign = 1;
    }
    else
    {
        sign = -1;
    }

    /*上一次的高频αβ轴分量*/
    hfi->alphabeta_hlast = hfi->alphabeta_h;

    /*提取这一次的高频αβ轴分量*/
    hfi->alphabeta_h = FOC_HFI_alphabeta_HF(alphabeta, hfi->alphabeta_last);
    /*提取这一次的低频分量*/
    hfi->alphabeta_f = FOC_HFI_alphabeta_LF(alphabeta, hfi->alphabeta_last);

    hfi->alphabeta_last = alphabeta; /*保存这次加了激励的值*/

    // /*这里求αβ轴电流的微分*/
    alphabeta_Ih.i_alpha = (hfi->alphabeta_h.i_alpha - hfi->alphabeta_hlast.i_alpha) * (float)sign;
    alphabeta_Ih.i_beta = (hfi->alphabeta_h.i_beta - hfi->alphabeta_hlast.i_beta) * (float)sign;

#if 0
    /*直接反正切*/
    hfi->speed.ElAngle = Limit_Angle(atan2f(alphabeta_Ih.beta, alphabeta_Ih.alpha));

#else
    /*高频注入锁相环*/
    PLL_Cale_HFI(&hfi->pll, alphabeta_Ih.i_alpha, alphabeta_Ih.i_beta);
    hfi->Encoder.ElAngle = Limit_Angle(hfi->pll.Angle);
// hfi->speed.ElAngle = Limit_Angle(hfi->pll.Theta + PI);
#endif
}

/*高频注入的转速计算*/
void FOC_HFI_Speed_Calc(FOC_HFI_t* hfi)
{
    hfi->Encoder.MecSpeed =
        Move_Filter_calculate(&hfi->pll.move_filter) / (float)hfi->Encoder.pole_pairs * 60.0f / M_2PI;
}

/*高频参数清零*/
void FOC_HFI_Clear(FOC_HFI_t* hfi)
{
    PLL_Clear(&hfi->pll);
}
